# coding=utf-8
# 2020/4/1: 导入bi_common

import bi_common as bi
from math import pi, sin, cos, atan2


# 可行驶区域序列的点集
class FreespaceSequencePoints:
    points = []  # 点的列表，单位m，按x1,y1,x2,y2...排列

    def __init__(self):
        pass


# 可行驶区域样本
class FreespaceSample:
    def __init__(self):
        self.sequences = []  # 可行驶区域序列(FreespaceSequencePoints)的列表，可多段描述

    # 通用样本，用于样本输出
    def to_general_sample(self, channel):
        output = bi.agency.create_general_sample()
        output.protocol = "freespace-sample-v1@" + str(channel)
        output.time = self.time
        sequence_points = 0
        sequence_offsets = []
        sequence_sizes = []
        for seq in self.sequences:
            point_count = len(seq.points) / 2
            sequence_offsets.append(sequence_points)
            sequence_sizes.append(point_count)
            sequence_points += point_count
        output.significant = 1 + 2 * (len(self.sequences) + sequence_points)
        output.values = []
        output.values.append(len(self.sequences))
        for i in range(0, len(self.sequences)):
            output.values.append(sequence_offsets[i])
            output.values.append(sequence_sizes[i])
            i += 1
        for seq in self.sequences:
            output.values += seq.points
        return output


def _conv_freespace_sample_v1(gs):
    values_count = len(gs.values)
    if values_count < 1:
        return None
    seq_count = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count < 1 + 2 * seq_count:
        return None
    pt_base = 1 + 2 * seq_count
    output = FreespaceSample()
    output.time = gs.time
    for i in range(0, seq_count):
        seq = FreespaceSequencePoints()
        b = 1 + 2 * i
        seq_ok = False
        seq_offset = 0
        seq_size = 0
        if gs.values[b] is not None and gs.values[b + 1] is not None:
            seq_offset = int(gs.values[b])
            seq_size = int(gs.values[b + 1])
            seq_ok = True
        if seq_ok:
            seq.points = gs.values[(pt_base + seq_offset * 2):(pt_base + (seq_offset + seq_size) * 2)]
        output.sequences.append(seq)
    return output


# 获取FreespaceSample，用于样本输入
def get_freespace_sample(channel):
    s1 = None
    s2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id = 'freespace-sample-v1@' + str(channel)
    if protocol_id in bi.input_samples:
        pair = bi.input_samples[protocol_id]
        s1 = _conv_freespace_sample_v1(pair.sample1)
        w1 = pair.weight1
        s2 = _conv_freespace_sample_v1(pair.sample2)
        w2 = pair.weight2
    if s1 is not None and s2 is not None:
        return s1 if w1 > w2 else s2
    return None
